#ifndef _UBT_STATE_TASK_NAV2_H_
#define _UBT_STATE_TASK_NAV2_H_
#include "ubt_interface/msg/ubt_com_msg.hpp"
#include "ubt_interface/msg/nav_path_info.hpp"
#include "ubt_interface/msg/line.hpp"
#include "ubt_interface/msg/limit.hpp"
#include "ubt_interface/msg/point.hpp"
#include "ubt_interface/msg/nav_task_limit.hpp"
#include "ubt_interface/msg/rfid.hpp"
#include "geometry_msgs/msg/twist.hpp"

#include "ubt_interface/srv/set_stop_distance.hpp"
#include "ubt_com/ubt_proto_nav.h"

#include "ubt_state_types.h"
#include "ubt_state_api.h"

// #include "geometry_msgs/msg/pose2_d.hpp"

namespace ubt_state
{
class CTaskNav2: public ITask
{
  public:
    static int ClassInit(ObstacleParams obsParams);
    static void ClassCleanup();
    static ITask *CreateTask(ITaskCb *pTaskCb, std::string taskId, std::vector<Line> *pLineInfos, 
      ubt_task_stage stage, int prePathAvoidHwMask, int index, int reportIndex);

    // 本体调度回充
    static ITask *CreateTask(ITaskCb *pTaskCb, std::string taskId, std::vector<Line> *pLineInfos, 
      ubt_task_stage stage, int prePathAvoidHwMask, int index, int reportIndex, int chargeExLoop);

    ~CTaskNav2();

    bool Init(ITaskCb *pTaskCb, std::string taskId, std::vector<Line> *pLineInfos, 
      ubt_task_stage stage, int prePathAvoidHwMask, int index, int reportIndex, int chargeExLoop);

    virtual bool Start();
    virtual void Pause();
    virtual void Continue();
    virtual void Stop(int reason);
    virtual void OnTimer();
    virtual void OnMsg(int msgType, void *pvMsg);

    virtual ubt_task_type GetType();
    virtual ubt_task_state GetState();
    virtual ubt_task_stage GetStage();
    virtual std::string GetActionType();
    virtual void GetIndex(int& index, int& reportIndex);
    virtual void GetStopInfo(int& stopReason, std::string& stopReasonDesc);
    virtual void GetLogs(std::vector<TaskLogItem>& logs);
    void Cmd_Vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg);
    void Smooth_Vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg);

    void OnRecvNavMsg(const ubt_interface::msg::UbtComMsg& msg, void *pvMsg);
    void OnSiteChanged(int pathIndex);
    void SetStateWrap(ubt_task_state newState, int reason, std::string reasonStr);
    void OnRecvNavRfidWrap(ubt_interface::msg::Rfid::SharedPtr  msg);
    int controlType;

  private:



    void _ApplyLineProp(int pathIndex);
    void _CheckObstacle(int event);
    void _SetState(ubt_task_state newState, int reason, std::string reasonStr);
    bool _HasSendStart();
    int  _SendStartReq();
    void _SendPauseReq();
    void _SendContinueReq();
    void _SendStopReq(int reason);
    void _PubNavLimit(NavTaskLimitIndex *taskLimit);
    void _OnLocationCb();
    void _OnAvoidStatusCb();
    void _OnArrive(int pathIndex);

    ITaskCb *_pTaskCb;
    std::string _taskId;
    int _index;
    int _reportIndex;
    std::vector<Line> *_pLineInfos;
    ubt_task_stage _stage;

    ubt_task_state _state;
    long _stateTime;
    int _flag;//0x01 startreq sended
    int _waitPathIndex;
    int _waitPointId;
    int _waitCmd;
    //标志位为1时等待，0不用等待
    int _wait_rfid;//rfid上报等待下发标志位，在导航中间件处发-1，（解决先读到rfid，会导致点位漏放问题）


    long _lastStatusTime;
    long _lastStatusQueryTime;

    int _stopReason;
    std::string _stopReasonDesc;

    int  _prePathAvoidHwMask;
    int  _obstacleState;//避障状态,0-不限,1 急停，2减速 ，3 报警
    int  _avoidHwStatus;	//硬件避障状态

    int _pathIndex;//当前线段
    int _lastPtId;//刚过的点
    int _nextPtId;//将去的点

    float current_speed;//当前线速度
    float current_angular;//当前角速度
    int * _point_status; //保存是否上报各点，做补发机制用，slam和磁条切换时容易掉
    std::string _current_pose_type;
    std::string _agv_type;
    
    int _chargeExLoop; // 回充轮次
};
}
#endif

